#pragma once
#include "DataDef.h"
#include "Derivative.h"
#include "DRState.h"


namespace Network
{

	static const float L_THRESHOLD = 10.0f;
	static const float FORCE = 10.0f;
	static const float C_TIME = 1.00f;
	static const float MOVE_SCALE = 30.0f;

	class DeadReck
	{
	public:
		DeadReck(void)
		{
			time = 0;
			sourcePosition = new D3DXVECTOR2(0, 0);
			remotePosition = new D3DXVECTOR2(0, 0);
			sourceDerivatives = new Derivative(D3DXVECTOR2(0, 0), D3DXVECTOR2(0, 0));

			lastStateSent = 
				new DRState(
				D3DXVECTOR2(*sourcePosition),
				time,
				Derivative(*sourceDerivatives));

			remotePreviousState = 
				new DRState(
				D3DXVECTOR2(lastStateSent->pos),
				lastStateSent->time,
				Derivative((*lastStateSent).deriv));

			remoteCurrentState =
				new DRState(
				D3DXVECTOR2(lastStateSent->pos),
				lastStateSent->time,
				Derivative((*lastStateSent).deriv));
		}

		~DeadReck(void)
		{
			delete sourcePosition;
			delete remotePosition;
			delete sourceDerivatives;
			delete lastStateSent;
			delete remotePreviousState;
			delete remoteCurrentState;
		}

		void update(const FLOAT timeStep)
		{
			// Process input
			D3DXVECTOR2 accel(0.0f, 0.0f);

			if( ::GetAsyncKeyState('I') & 0x8000f )
				accel.y = -FORCE;

			if( ::GetAsyncKeyState('K') & 0x8000f )
				accel.y = FORCE;

			if( ::GetAsyncKeyState('J') & 0x8000f )
				accel.x = FORCE;

			if( ::GetAsyncKeyState('L') & 0x8000f )
				accel.x = -FORCE;

			// Update source DRState
			sourceDerivatives->accel = accel;

			D3DXVECTOR2 prev(sourcePosition->x, sourcePosition->y);
			integrate(*sourcePosition, *sourceDerivatives, timeStep);

			if (sourcePosition->x < -MOVE_SCALE)
			{
				sourceDerivatives->vel.x = 0.0f;
				sourcePosition->x = prev.x;
			}
			if (sourcePosition->x > MOVE_SCALE)
			{
				sourceDerivatives->vel.x = 0.0f;
				sourcePosition->x = prev.x;
			}
			if (sourcePosition->y > MOVE_SCALE)
			{
				sourceDerivatives->vel.y = 0.0f;
				sourcePosition->y = prev.y;
			}
			if (sourcePosition->y < -MOVE_SCALE)
			{
				sourceDerivatives->vel.y = 0.0f;
				sourcePosition->y = prev.y;
			}

			// Send an update if DR threshold exceeded
			DRState *update = deadReckoningMessage(*sourcePosition, time, *sourceDerivatives, *lastStateSent);
			if (update != NULL) {
				send(update);
			}

			// Predict remote position
			deadReckoningValue(remotePosition, remotePreviousState, remoteCurrentState, time);

			time += timeStep;
		}

		/////////////////////////////////////////////////////////

		void send(DRState *update)
		{
			lastStateSent = update;
			D3DXVECTOR2 predOut;
			prediction(&predOut, remoteCurrentState, time);
			remotePreviousState = new DRState(
				predOut,
				time,
				Derivative((*remoteCurrentState).deriv));
			remoteCurrentState = new DRState(
				D3DXVECTOR2(update->pos),
				update->time,
				Derivative((*update).deriv));
		}

		void deadReckoningValue(D3DXVECTOR2 *out, const DRState *previous, const DRState *current, const FLOAT t)
		{
			// Linear convergence
			FLOAT startTime = previous->time;
			FLOAT endTime = startTime + C_TIME;

			if (t < endTime)
			{
				D3DXVECTOR2 startPos = previous->pos;
				D3DXVECTOR2 endPos;
				prediction(&endPos, remoteCurrentState, time);
				convergenceLinear(out, &startPos, startTime, &endPos, endTime, t);
			}
			else
			{
				prediction(out, remoteCurrentState, time);
			}
		}

		void integrate(D3DXVECTOR2 &pos, Derivative &deriv, FLOAT d)
		{
			D3DXVec2Subtract(&deriv.vel, &deriv.vel, &(deriv.accel * d));
			D3DXVec2Add(&pos, &pos, &(deriv.vel * d));
		}

		DRState *deadReckoningMessage(D3DXVECTOR2 pos, FLOAT t, Derivative deriv, DRState last)
		{
			D3DXVECTOR2 out;
			D3DXVECTOR2 predPos;
			prediction(&predPos, remoteCurrentState, time);
			D3DXVec2Subtract(&out, &pos, &predPos);
			D3DXVec2Length(&out);
			if(D3DXVec2Length(&out) < L_THRESHOLD)
			{
				return NULL;
			}
			DRState *u = new DRState(D3DXVECTOR2(pos), t, Derivative(deriv));
			return u;
		}

		void convergenceLinear(D3DXVECTOR2 *out, const D3DXVECTOR2 *p0, const FLOAT t0, const D3DXVECTOR2 *p1, const FLOAT t1, const FLOAT t)
		{
			FLOAT d = (t - t0) / (t1 - t0);
			D3DXVec2Subtract(out, p1, p0);
			*out *= d;
			D3DXVec2Add(out, out, p0);
		}

		void prediction(D3DXVECTOR2 *out, const DRState *state, const FLOAT t)
		{
			FLOAT d = t - state->time;
			D3DXVECTOR2 v = state->deriv.vel;
			// pos + vd
			D3DXVec2Add(out, &state->pos, &(v * d));
		}


		D3DXVECTOR2 *sourcePosition;
		D3DXVECTOR2 *remotePosition;

	private:
		FLOAT time;
		Derivative *sourceDerivatives;
		DRState *lastStateSent;
		DRState *remotePreviousState;
		DRState *remoteCurrentState;
	};
}